#include "WProgram.h"
// Experiment with different ways to measure tilt angle
#define BAUD 38400  

// Define pin assignments
#define LEDPIN 13
#define ROLLRATEPIN 4
#define PITCHRATEPIN 3
#define YAWRATEPIN 5

// Gyro setup
// if AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// float GyroMid = 465;
// gyro range is from +/-500 deg/sec
// float GyroScaleFactor = 1001 / (float)ZMAX;
void setup();
void loop();
int digitalSmooth(int rawIn, int *sensSmoothArray);
int xGyroZero, yGyroZero, zGyroZero;
int rollRate, pitchRate, yawRate;

unsigned long previousTime = 0;
unsigned long deltaTime = 0;

#define FILTERSAMPLES 23              // filterSamples should  be an odd number, no smaller than 3
int rollSmoothArray [FILTERSAMPLES];   // array for holding raw sensor values for roll 
int rollData, rollSmoothData;          // variables for roll data
int pitchSmoothArray [FILTERSAMPLES];  // array for holding raw sensor values for pitch 
int pitchData, pitchSmoothData;        // variables for pitch data
int yawSmoothArray [FILTERSAMPLES];    // array for holding raw sensor values for yaw 
int yawData, yawSmoothData;            // variables for yaw data

void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  pinMode(ROLLRATEPIN, INPUT);
  pinMode(PITCHRATEPIN, INPUT);
  pinMode(YAWRATEPIN, INPUT);
  digitalWrite(LEDPIN, HIGH);
  previousTime = millis();
}

void loop() {
  previousTime = millis();
  rollRate = digitalSmooth(analogRead(ROLLRATEPIN), rollSmoothArray);
  pitchRate = digitalSmooth(analogRead(PITCHRATEPIN), pitchSmoothArray);
  yawRate = digitalSmooth(analogRead(YAWRATEPIN), yawSmoothArray);
  deltaTime= millis() - previousTime;

  Serial.print(deltaTime);
  Serial.print(',');
  Serial.print(rollRate);
  Serial.print(',');
  Serial.print(pitchRate);
  Serial.print(',');
  Serial.println(yawRate);

}

int digitalSmooth(int rawIn, int *sensSmoothArray) {
  // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  long total;
  static int i;
  static int sorted[FILTERSAMPLES];
  boolean done;

  i = (i + 1) % FILTERSAMPLES; // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn; // input new data into the oldest slot

  for (j=0; j<FILTERSAMPLES; j++){ // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (FILTERSAMPLES - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }
  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((FILTERSAMPLES * 15) / 100), 1); 
  top = min((((FILTERSAMPLES * 85) / 100) + 1), (FILTERSAMPLES - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}


int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

